'''
Author: your name
Date: 2021-07-29 22:00:18
LastEditTime: 2021-07-29 22:00:19
LastEditors: your name
Description: In User Settings Edit
FilePath: \MyOpenmv\Optical_and_blob\main.py
'''
'''
Author: your name
Date: 2021-07-29 22:00:18
LastEditTime: 2021-07-29 22:00:18
LastEditors: your name
Description: In User Settings Edit
FilePath: \MyOpenmv\Optical_and_blob\main.py
'''
import sensor,time,pyb

from OpticalFlow import flowing
from pyb import Pin, Timer, LED, UART
#黑色点阈值

yellow_threshold = [(44, 95, -56, 53, 23, 127)]
green_led = pyb.LED(2)
red_led = pyb.LED(1)
blue_led = pyb.LED(3)

uart = UART(3,115200)
sensor.reset() # Initialize the camera sensor.

sensor.set_pixformat(sensor.RGB565) # or sensor.GRAYSCALE

sensor.set_framesize(sensor.HQVGA)

sensor.skip_frames(20)#相机自检几张图片


clock = time.clock()#打开时钟

def pack_test_data(x, y, x1, y1, ok):

    pack_data=bytearray([x,y,x1,y1,ok,0X00])

    lens = len(pack_data)#数据包大小


    i = 0
    sum = 0

    #和校验
    while i<(lens-1):
        sum = sum + pack_data[i]
        i = i+1
    pack_data[lens-1] = sum

    return pack_data

(lastX,lastY) = (120, 80)
(maxOptX, maxOptY) = (0, 0)
findCount = 0
lastTime = 0
while(True):

    clock.tick()

    img = sensor.snapshot()



    blobs = img.find_blobs(yellow_threshold)
    (X, Y) = (255, 255)
    (optX,optY, optOK) = flowing(img)
   # car.run(50,50)
    if blobs:
        findCount = 50
        blue_led.off()
        green_led.on()
        red_led.off()
        most_pixels = 0
        largest_blob = 0
        for i in range(len(blobs)):
            if blobs[i].pixels() > most_pixels:
                most_pixels = blobs[i].pixels()
                largest_blob = i
        (X, Y) = (blobs[largest_blob].cx(),blobs[largest_blob].cy())

        #img.draw_cross(blobs[largest_blob].cx(),blobs[largest_blob].cy())

        #img.draw_rectangle(blobs[largest_blob].rect())
        (lastX, lastY) = (X, Y)
    else:
        if findCount:
            findCount -= 1
            blue_led.on()
            red_led.off()
            green_led.off()
        else:
            blue_led.off()
            red_led.on()
            green_led.off()
#等待主机命令，相当于同步指令
    #while(not uart.any()):
       # pass

    if uart.readchar() == 0xAA:
#如果未找到色块，（X，Y） == （255，255）
        if (X,Y) == (255,255):
#未找到色块时即丢失数据时，设置findCount初始值，若在反应时间以内，仍然发送上一次检测到的位置值，若超出反应时间，即发送（255，255）
            if findCount:
                uart.write(pack_test_data(159 - lastY, 239 - lastX, optX, optY, optOK ))
            else:
                uart.write(pack_test_data(255, 255, optX, optY, optOK ))
#找到色块，发送位置值
        else:
            uart.write(pack_test_data(159 - Y, 239 - X, optX, optY, optOK ))
    #print(pyb.elapsed_millis(lastTime))
    #lastTime = pyb.millis()
    (maxOptX, maxOptY) = (max(maxOptX, abs(optX)), max(maxOptY, abs(optY)))
    print('%d %d %d %d %d' % (maxOptX, maxOptX, optX, optY, optOK))

